#ifndef __VELOCITY_PLANNING_BASE_H_
#define __VELOCITY_PLANNING_BASE_H_

#include <mc_common/base_define.h>
#include <mc_common/msg_convert.h>
#include <mc_common/math_utils.h>
#include <mc_common/path_utils.h>
#include <mc_map/map_base.h>

// #include <mc_velocity_planning/move_base/move_pose.h>
// #include <mc_velocity_planning/pure_suit/pure_suit.h>

using namespace std;

enum VelocityPlanningType{
    DWAPlanner = 1,
    PureSuitPlanner = 2,
    PIDPlanner = 3,
    MPCPlanner = 4,
};

class VelocityPlanningBase : public MapBase{
    public:
        VelocityPlanningBase(tf2_ros::Buffer* tf_buffer);
        ~VelocityPlanningBase();

        // 速度规划  相对于base_link
        virtual bool ComputeTwist(Velocity2D curr_vel, Velocity2D& next_vel, Pose2D local_goal) = 0;

        // 设置雷达点云数据  相对于base_link
        void setPointCloudData(sensor_msgs::PointCloud2::Ptr cloud_ptr);
        // 检测目标点是否到达  距离判断
        bool IsPositionReached(Pose2D robot_pose, Pose2D goal_pose, float xy_tolerance);
        // 检测目标点是否到达  角度判断
        bool IsAngleReached(Pose2D robot_pose, Pose2D goal_pose, float yaw_tolerance);
        // 获取离当前位置最近的前瞻点  简单的距离计算，后期优化
        bool FindLookAheadPointByDistance(vector<Pose2D>& local_path, 
            Pose2D curr, Pose2D& target_point, float lookahead_distance);
        // 发布空速度
        void PublishEmptyTwist();
        // 发布速度
        void PublishTwist(Velocity2D velocity);
        // 机器人停止
        bool Stop(Velocity2D& cmd_vel, bool e_stop, Velocity2D curr_vel, Pose2D curr, Pose2D goal);
        // 旋转到目标点位姿 的周期速度
        bool RotateToGoal(Velocity2D& cmd_vel, Pose2D curr, Pose2D goal);
        // 获取机器人当前位姿
        bool GetRobotPose(geometry_msgs::PoseStamped& global_pose);
        // 获取TF坐标变换
        bool GetTFPose(geometry_msgs::PoseStamped& global_pose, 
            string father_frame, string child_frame, ros::Time stamp);

        // 符号判断
        inline float sign(float x){
            return x < 0.0 ? -1.0 : 1.0;
        }

    private:
        ros::NodeHandle nh_;
        ros::Publisher velPub_;
        ros::Publisher localPathPub_;
        ros::Publisher LookaheadPosePub_;
        tf2_ros::Buffer* const tfBuffer_;

    protected:
        Velocity2D __max_vel_limit = Velocity2D(0.8, 0.8, 1.2);       //最大速度限制
        Velocity2D __min_vel_limit = Velocity2D(-0.8, -0.8, -1.2);    //最小速度限制
        float __transform_tolerance = 0.2;
        string __base_frame = "base_link";      //局部坐标系 一般为base_link
        string __map_frame = "map";             //全局坐标系 一般为map
        float __controller_frequency = 0.05;   //20Hz

        sensor_data::PointCloudXYZPtr pointCloudXYZPtr_;    //点云数据
};

#endif